#include <vector>
#include <glm\glm.hpp>

class DistanceFunction
{
	public:

	//************************************
	// Method:    dead_reckoning
	// Description: Takes a binary image and finds its edges (borderpoints) and distance to closest binary image pixel.
	// Parameter: const std::vector< std::vector< unsigned char > > & map
	// Parameter: std::vector< std::vector< float > > & distmap // output
	// Parameter: std::vector< std::vector< Point > > & borderpoint
	//************************************
	static void dead_reckoning(const std::vector< std::vector< bool > > &map, 
						std::vector< std::vector< float > > &distmap, 
						std::vector< std::vector< glm::vec2 > > &borderpoint,
						int xmin, int ymin, int xmax, int ymax)

	{
		xmin = std::max<int>( xmin, 1 );
		ymin = std::max<int>( ymin, 1 );
		xmax = std::min<int>( xmax, borderpoint.size()-2 );
		ymax = std::min<int>( ymax, borderpoint.size()-2 );


		//float eps = 1e-6f;
		float d2 = sqrt(2.0f); // diagonal distance

		// initialize distance map
		for(int i=xmin-1; i<xmax+1; i++)
		{
			for(int j=ymin-1; j<ymax+1; j++)
			{
				//if( !map[i][j] )
				{
					distmap [i][j] = 1e6f;
					borderpoint[i][j].x = -1;
					borderpoint[i][j].y = -1;
				}
			}
		}

		// initialize interior & exterior elements
		for(int j=ymin; j<ymax; j++)
		{
			for(int i=xmin; i<xmax; i++)
			{
				//if( fabs( map[i-1][j] - map[i][j]) > eps || fabs(map[i+1][j] - map[i][j]) > eps ||
				//	fabs( map[i][j-1] - map[i][j]) > eps || fabs(map[i][j+1] - map[i][j]) > eps)      

				if( (map[i-1][j] != map[i][j] || map[i+1][j] != map[i][j])  ||
					(map[i][j-1] != map[i][j]) || map[i][j+1] != map[i][j] )
				{
					distmap [i][j] = 0;
					borderpoint[i][j].x = i;
					borderpoint[i][j].y = j;
				}
			}
		}

		// perform first pass
		for(int j=ymin; j<ymax; j++)
		{
			for(int i=xmin; i<xmax; i++)
			{
				if( (distmap[i-1][j-1] + d2 < distmap[i][j]) )
				{
					borderpoint[i][j].x = borderpoint[i-1][j-1].x;
					borderpoint[i][j].y = borderpoint[i-1][j-1].y;
					int px = borderpoint[i][j].x;
					int py = borderpoint[i][j].y;
					distmap [i][j] = sqrt( (float)(i-px)*(i-px) + (float)(j-py)*(j-py) );
				}

				if( (distmap[i][j-1] + 1 < distmap[i][j]) )
				{
					borderpoint[i][j].x = borderpoint[i][j-1].x;
					borderpoint[i][j].y = borderpoint[i][j-1].y;         
					int px = borderpoint[i][j].x;
					int py = borderpoint[i][j].y;
					distmap [i][j] = sqrt( (float)(i-px)*(i-px) + (float)(j-py)*(j-py) );
				}

				if( (distmap[i+1][j-1] + d2 < distmap[i][j]) )
				{
					borderpoint[i][j].x = borderpoint[i+1][j-1].x;
					borderpoint[i][j].y = borderpoint[i+1][j-1].y;
					int px = borderpoint[i][j].x;
					int py = borderpoint[i][j].y;
					distmap [i][j] = sqrt( (float)(i-px)*(i-px) + (float)(j-py)*(j-py) );
				}

				if( (distmap[i-1][j] + 1 < distmap[i][j]) )
				{
					borderpoint[i][j].x = borderpoint[i-1][j].x;
					borderpoint[i][j].y = borderpoint[i-1][j].y;  
					int px = borderpoint[i][j].x;
					int py = borderpoint[i][j].y;
					distmap [i][j] = sqrt( (float)(i-px)*(i-px) + (float)(j-py)*(j-py) );
				}

			}
		}


		// perform final pass, note iteration dir
		for(int j=ymax; j>ymin; --j)
		{
			for(int i=xmax; i>xmin; --i)
			{
				if( (distmap[i+1][j] + 1 < distmap[i][j]) )
				{
					borderpoint[i][j].x = borderpoint[i+1][j].x;
					borderpoint[i][j].y = borderpoint[i+1][j].y;
					int px = borderpoint[i][j].x;
					int py = borderpoint[i][j].y;
					distmap [i][j] = sqrt( (float)(i-px)*(i-px) + (float)(j-py)*(j-py) );
				}

				if( (distmap[i-1][j+1] + d2 < distmap[i][j]) )
				{
					borderpoint[i][j].x = borderpoint[i-1][j+1].x;
					borderpoint[i][j].y = borderpoint[i-1][j+1].y;         
					int px = borderpoint[i][j].x;
					int py = borderpoint[i][j].y;
					distmap [i][j] = sqrt( (float)(i-px)*(i-px) + (float)(j-py)*(j-py) );
				}

				if( (distmap[i][j+1] + 1 < distmap[i][j]) )
				{
					borderpoint[i][j].x = borderpoint[i][j+1].x;
					borderpoint[i][j].y = borderpoint[i][j+1].y;
					int px = borderpoint[i][j].x;
					int py = borderpoint[i][j].y;
					distmap [i][j] = sqrt( (float)(i-px)*(i-px) + (float)(j-py)*(j-py) );
				}

				if( (distmap[i+1][j+1] + d2 < distmap[i][j]) )
				{
					borderpoint[i][j].x = borderpoint[i+1][j+1].x;
					borderpoint[i][j].y = borderpoint[i+1][j+1].y;  
					int px = borderpoint[i][j].x;
					int py = borderpoint[i][j].y;
					distmap [i][j] = sqrt( (float)(i-px)*(i-px) + (float)(j-py)*(j-py) );;
				}

			}
		}

		for(int i=xmin; i<xmax; i++)
		{
			for(int j=ymin; j<ymax; j++)
			{
				//if( map[i][j] == false ) distmap[i][j] *= -1;
				//distmap[i][j] *= -1;
				distmap[i][j] =  distmap[i][j] / distmap.size() * 60.0f;
				//distmap[i][j] = distmap[i][j]*distmap[i][j];
				distmap[i][j] = std::min( 0.5f, distmap[i][j] );
			}
		}

	}
};